% 
% LibQPEP: A Library for Globally Optimal Solving Quadratic Pose Estimation Problems (QPEPs),
%          It also gives highly accurate uncertainty description of the solutions.
%
%
% Article: 
%      Wu, J., Zheng, Y., Gao, Z., Jiang, Y., Hu, X., Zhu, Y., Jiao, J., Liu, M. (2020)
%           Quadratic Pose Estimation Problems: Unified Solutions, 
%           Solvability/Observability Analysis and Uncertainty Description 
%           in A Globally Optimal Framework.
%
%
% Authors:      Jin Wu and Ming Liu
% Affiliation:  Hong Kong University of Science and Technology (HKUST)
% Emails:       jin_wu_uestc@hotmail.com; eelium@ust.hk
% Websites:     https://zarathustr.github.io
%               https://ram-lab.com



function [R, t, X, min, qs] = QPEP_WQ_grobner(W, Q, solver_func, mon_J_pure_func, t_funcs, coef_J_pure, coefs_tq, pinvG, params)
t1_func = t_funcs{1};
t2_func = t_funcs{2};
t3_func = t_funcs{3};
J_func = @(q, t)(coef_J_pure * mon_J_pure_func(q, t).');
t_func = @(q)([
    t1_func(pinvG, coefs_tq, q);
    t2_func(pinvG, coefs_tq, q);
    t3_func(pinvG, coefs_tq, q);
    ]);

if(strcmp(char(solver_func), char(@solver_Wu_Qq)))
    W1_1 = W(1, 1); W1_2 = W(1, 2); W1_3 = W(1, 3); W1_4 = W(1, 4); W1_5 = W(1, 5); W1_6 = W(1, 6); W1_7 = W(1, 7); W1_8 = W(1, 8); W1_9 = W(1, 9); W1_10 = W(1, 10); W1_11 = W(1, 11); W1_12 = W(1, 12); W1_13 = W(1, 13); W1_14 = W(1, 14); W1_15 = W(1, 15); W1_16 = W(1, 16); W1_17 = W(1, 17); W1_18 = W(1, 18); W1_19 = W(1, 19); W1_20 = W(1, 20); W1_21 = W(1, 21); W1_22 = W(1, 22); W1_23 = W(1, 23); W1_24 = W(1, 24); W1_25 = W(1, 25); W1_26 = W(1, 26); W1_27 = W(1, 27); W1_28 = W(1, 28); W1_29 = W(1, 29); W1_30 = W(1, 30); W1_31 = W(1, 31); W1_32 = W(1, 32); W1_33 = W(1, 33); W1_34 = W(1, 34); W1_35 = W(1, 35); W1_36 = W(1, 36); W1_37 = W(1, 37); W1_38 = W(1, 38); W1_39 = W(1, 39); W1_40 = W(1, 40); W1_41 = W(1, 41); W1_42 = W(1, 42); W1_43 = W(1, 43); W1_44 = W(1, 44); W1_45 = W(1, 45); W1_46 = W(1, 46); W1_47 = W(1, 47); W1_48 = W(1, 48); W1_49 = W(1, 49); W1_50 = W(1, 50); W1_51 = W(1, 51); W1_52 = W(1, 52); W1_53 = W(1, 53); W1_54 = W(1, 54); W1_55 = W(1, 55); W1_56 = W(1, 56); W1_57 = W(1, 57); W1_58 = W(1, 58); W1_59 = W(1, 59); W1_60 = W(1, 60); W1_61 = W(1, 61); W1_62 = W(1, 62); W1_63 = W(1, 63); W1_64 = W(1, 64); W2_1 = W(2, 1); W2_2 = W(2, 2); W2_3 = W(2, 3); W2_4 = W(2, 4); W2_5 = W(2, 5); W2_6 = W(2, 6); W2_7 = W(2, 7); W2_8 = W(2, 8); W2_9 = W(2, 9); W2_10 = W(2, 10); W2_11 = W(2, 11); W2_12 = W(2, 12); W2_13 = W(2, 13); W2_14 = W(2, 14); W2_15 = W(2, 15); W2_16 = W(2, 16); W2_17 = W(2, 17); W2_18 = W(2, 18); W2_19 = W(2, 19); W2_20 = W(2, 20); W2_21 = W(2, 21); W2_22 = W(2, 22); W2_23 = W(2, 23); W2_24 = W(2, 24); W2_25 = W(2, 25); W2_26 = W(2, 26); W2_27 = W(2, 27); W2_28 = W(2, 28); W2_29 = W(2, 29); W2_30 = W(2, 30); W2_31 = W(2, 31); W2_32 = W(2, 32); W2_33 = W(2, 33); W2_34 = W(2, 34); W2_35 = W(2, 35); W2_36 = W(2, 36); W2_37 = W(2, 37); W2_38 = W(2, 38); W2_39 = W(2, 39); W2_40 = W(2, 40); W2_41 = W(2, 41); W2_42 = W(2, 42); W2_43 = W(2, 43); W2_44 = W(2, 44); W2_45 = W(2, 45); W2_46 = W(2, 46); W2_47 = W(2, 47); W2_48 = W(2, 48); W2_49 = W(2, 49); W2_50 = W(2, 50); W2_51 = W(2, 51); W2_52 = W(2, 52); W2_53 = W(2, 53); W2_54 = W(2, 54); W2_55 = W(2, 55); W2_56 = W(2, 56); W2_57 = W(2, 57); W2_58 = W(2, 58); W2_59 = W(2, 59); W2_60 = W(2, 60); W2_61 = W(2, 61); W2_62 = W(2, 62); W2_63 = W(2, 63); W2_64 = W(2, 64); W3_1 = W(3, 1); W3_2 = W(3, 2); W3_3 = W(3, 3); W3_4 = W(3, 4); W3_5 = W(3, 5); W3_6 = W(3, 6); W3_7 = W(3, 7); W3_8 = W(3, 8); W3_9 = W(3, 9); W3_10 = W(3, 10); W3_11 = W(3, 11); W3_12 = W(3, 12); W3_13 = W(3, 13); W3_14 = W(3, 14); W3_15 = W(3, 15); W3_16 = W(3, 16); W3_17 = W(3, 17); W3_18 = W(3, 18); W3_19 = W(3, 19); W3_20 = W(3, 20); W3_21 = W(3, 21); W3_22 = W(3, 22); W3_23 = W(3, 23); W3_24 = W(3, 24); W3_25 = W(3, 25); W3_26 = W(3, 26); W3_27 = W(3, 27); W3_28 = W(3, 28); W3_29 = W(3, 29); W3_30 = W(3, 30); W3_31 = W(3, 31); W3_32 = W(3, 32); W3_33 = W(3, 33); W3_34 = W(3, 34); W3_35 = W(3, 35); W3_36 = W(3, 36); W3_37 = W(3, 37); W3_38 = W(3, 38); W3_39 = W(3, 39); W3_40 = W(3, 40); W3_41 = W(3, 41); W3_42 = W(3, 42); W3_43 = W(3, 43); W3_44 = W(3, 44); W3_45 = W(3, 45); W3_46 = W(3, 46); W3_47 = W(3, 47); W3_48 = W(3, 48); W3_49 = W(3, 49); W3_50 = W(3, 50); W3_51 = W(3, 51); W3_52 = W(3, 52); W3_53 = W(3, 53); W3_54 = W(3, 54); W3_55 = W(3, 55); W3_56 = W(3, 56); W3_57 = W(3, 57); W3_58 = W(3, 58); W3_59 = W(3, 59); W3_60 = W(3, 60); W3_61 = W(3, 61); W3_62 = W(3, 62); W3_63 = W(3, 63); W3_64 = W(3, 64); W4_1 = W(4, 1); W4_2 = W(4, 2); W4_3 = W(4, 3); W4_4 = W(4, 4); W4_5 = W(4, 5); W4_6 = W(4, 6); W4_7 = W(4, 7); W4_8 = W(4, 8); W4_9 = W(4, 9); W4_10 = W(4, 10); W4_11 = W(4, 11); W4_12 = W(4, 12); W4_13 = W(4, 13); W4_14 = W(4, 14); W4_15 = W(4, 15); W4_16 = W(4, 16); W4_17 = W(4, 17); W4_18 = W(4, 18); W4_19 = W(4, 19); W4_20 = W(4, 20); W4_21 = W(4, 21); W4_22 = W(4, 22); W4_23 = W(4, 23); W4_24 = W(4, 24); W4_25 = W(4, 25); W4_26 = W(4, 26); W4_27 = W(4, 27); W4_28 = W(4, 28); W4_29 = W(4, 29); W4_30 = W(4, 30); W4_31 = W(4, 31); W4_32 = W(4, 32); W4_33 = W(4, 33); W4_34 = W(4, 34); W4_35 = W(4, 35); W4_36 = W(4, 36); W4_37 = W(4, 37); W4_38 = W(4, 38); W4_39 = W(4, 39); W4_40 = W(4, 40); W4_41 = W(4, 41); W4_42 = W(4, 42); W4_43 = W(4, 43); W4_44 = W(4, 44); W4_45 = W(4, 45); W4_46 = W(4, 46); W4_47 = W(4, 47); W4_48 = W(4, 48); W4_49 = W(4, 49); W4_50 = W(4, 50); W4_51 = W(4, 51); W4_52 = W(4, 52); W4_53 = W(4, 53); W4_54 = W(4, 54); W4_55 = W(4, 55); W4_56 = W(4, 56); W4_57 = W(4, 57); W4_58 = W(4, 58); W4_59 = W(4, 59); W4_60 = W(4, 60); W4_61 = W(4, 61); W4_62 = W(4, 62); W4_63 = W(4, 63); W4_64 = W(4, 64); Q1_1 = Q(1, 1); Q1_2 = Q(1, 2); Q1_3 = Q(1, 3); Q1_4 = Q(1, 4); Q2_1 = Q(2, 1); Q2_2 = Q(2, 2); Q2_3 = Q(2, 3); Q2_4 = Q(2, 4); Q3_1 = Q(3, 1); Q3_2 = Q(3, 2); Q3_3 = Q(3, 3); Q3_4 = Q(3, 4); Q4_1 = Q(4, 1); Q4_2 = Q(4, 2); Q4_3 = Q(4, 3); Q4_4 = Q(4, 4); 
    [q0_, q1_, q2_, q3_] = solver_func(W1_1, W2_1, W3_1, W4_1, W1_2, W2_2, W3_2, W4_2, W1_3, W2_3, W3_3, W4_3, W1_4, W2_4, W3_4, W4_4, W1_5, W2_5, W3_5, W4_5, W1_6, W2_6, W3_6, W4_6, W1_7, W2_7, W3_7, W4_7, W1_8, W2_8, W3_8, W4_8, W1_9, W2_9, W3_9, W4_9, W1_10, W2_10, W3_10, W4_10, W1_11, W2_11, W3_11, W4_11, W1_12, W2_12, W3_12, W4_12, W1_13, W2_13, W3_13, W4_13, W1_14, W2_14, W3_14, W4_14, W1_15, W2_15, W3_15, W4_15, W1_16, W2_16, W3_16, W4_16, W1_17, W2_17, W3_17, W4_17, W1_18, W2_18, W3_18, W4_18, W1_19, W2_19, W3_19, W4_19, W1_20, W2_20, W3_20, W4_20, W1_21, W2_21, W3_21, W4_21, W1_22, W2_22, W3_22, W4_22, W1_23, W2_23, W3_23, W4_23, W1_24, W2_24, W3_24, W4_24, W1_25, W2_25, W3_25, W4_25, W1_26, W2_26, W3_26, W4_26, W1_27, W2_27, W3_27, W4_27, W1_28, W2_28, W3_28, W4_28, W1_29, W2_29, W3_29, W4_29, W1_30, W2_30, W3_30, W4_30, W1_31, W2_31, W3_31, W4_31, W1_32, W2_32, W3_32, W4_32, W1_33, W2_33, W3_33, W4_33, W1_34, W2_34, W3_34, W4_34, W1_35, W2_35, W3_35, W4_35, W1_36, W2_36, W3_36, W4_36, W1_37, W2_37, W3_37, W4_37, W1_38, W2_38, W3_38, W4_38, W1_39, W2_39, W3_39, W4_39, W1_40, W2_40, W3_40, W4_40, W1_41, W2_41, W3_41, W4_41, W1_42, W2_42, W3_42, W4_42, W1_43, W2_43, W3_43, W4_43, W1_44, W2_44, W3_44, W4_44, W1_45, W2_45, W3_45, W4_45, W1_46, W2_46, W3_46, W4_46, W1_47, W2_47, W3_47, W4_47, W1_48, W2_48, W3_48, W4_48, W1_49, W2_49, W3_49, W4_49, W1_50, W2_50, W3_50, W4_50, W1_51, W2_51, W3_51, W4_51, W1_52, W2_52, W3_52, W4_52, W1_53, W2_53, W3_53, W4_53, W1_54, W2_54, W3_54, W4_54, W1_55, W2_55, W3_55, W4_55, W1_56, W2_56, W3_56, W4_56, W1_57, W2_57, W3_57, W4_57, W1_58, W2_58, W3_58, W4_58, W1_59, W2_59, W3_59, W4_59, W1_60, W2_60, W3_60, W4_60, W1_61, W2_61, W3_61, W4_61, W1_62, W2_62, W3_62, W4_62, W1_63, W2_63, W3_63, W4_63, W1_64, W2_64, W3_64, W4_64, Q1_1, Q2_1, Q3_1, Q4_1, Q1_2, Q2_2, Q3_2, Q4_2, Q1_3, Q2_3, Q3_3, Q4_3, Q1_4, Q2_4, Q3_4, Q4_4);
elseif(strcmp(char(solver_func), char(@solver_Wu_Qq_approx)))
    perm = params{1};
    W = W(perm, :);
    Q = Q(perm, :);
    W1_1 = W(1, 1); W1_2 = W(1, 2); W1_3 = W(1, 3); W1_4 = W(1, 4); W1_5 = W(1, 5); W1_6 = W(1, 6); W1_7 = W(1, 7); W1_8 = W(1, 8); W1_9 = W(1, 9); W1_10 = W(1, 10); W1_11 = W(1, 11); W1_12 = W(1, 12); W1_13 = W(1, 13); W1_14 = W(1, 14); W1_15 = W(1, 15); W1_16 = W(1, 16); W1_17 = W(1, 17); W1_18 = W(1, 18); W1_19 = W(1, 19); W1_20 = W(1, 20); W1_21 = W(1, 21); W1_22 = W(1, 22); W1_23 = W(1, 23); W1_24 = W(1, 24); W1_25 = W(1, 25); W1_26 = W(1, 26); W1_27 = W(1, 27); W1_28 = W(1, 28); W1_29 = W(1, 29); W1_30 = W(1, 30); W1_31 = W(1, 31); W1_32 = W(1, 32); W1_33 = W(1, 33); W1_34 = W(1, 34); W1_35 = W(1, 35); W1_36 = W(1, 36); W1_37 = W(1, 37); W1_38 = W(1, 38); W1_39 = W(1, 39); W1_40 = W(1, 40); W1_41 = W(1, 41); W1_42 = W(1, 42); W1_43 = W(1, 43); W1_44 = W(1, 44); W1_45 = W(1, 45); W1_46 = W(1, 46); W1_47 = W(1, 47); W1_48 = W(1, 48); W1_49 = W(1, 49); W1_50 = W(1, 50); W1_51 = W(1, 51); W1_52 = W(1, 52); W1_53 = W(1, 53); W1_54 = W(1, 54); W1_55 = W(1, 55); W1_56 = W(1, 56); W1_57 = W(1, 57); W1_58 = W(1, 58); W1_59 = W(1, 59); W1_60 = W(1, 60); W1_61 = W(1, 61); W1_62 = W(1, 62); W1_63 = W(1, 63); W1_64 = W(1, 64); W2_1 = W(2, 1); W2_2 = W(2, 2); W2_3 = W(2, 3); W2_4 = W(2, 4); W2_5 = W(2, 5); W2_6 = W(2, 6); W2_7 = W(2, 7); W2_8 = W(2, 8); W2_9 = W(2, 9); W2_10 = W(2, 10); W2_11 = W(2, 11); W2_12 = W(2, 12); W2_13 = W(2, 13); W2_14 = W(2, 14); W2_15 = W(2, 15); W2_16 = W(2, 16); W2_17 = W(2, 17); W2_18 = W(2, 18); W2_19 = W(2, 19); W2_20 = W(2, 20); W2_21 = W(2, 21); W2_22 = W(2, 22); W2_23 = W(2, 23); W2_24 = W(2, 24); W2_25 = W(2, 25); W2_26 = W(2, 26); W2_27 = W(2, 27); W2_28 = W(2, 28); W2_29 = W(2, 29); W2_30 = W(2, 30); W2_31 = W(2, 31); W2_32 = W(2, 32); W2_33 = W(2, 33); W2_34 = W(2, 34); W2_35 = W(2, 35); W2_36 = W(2, 36); W2_37 = W(2, 37); W2_38 = W(2, 38); W2_39 = W(2, 39); W2_40 = W(2, 40); W2_41 = W(2, 41); W2_42 = W(2, 42); W2_43 = W(2, 43); W2_44 = W(2, 44); W2_45 = W(2, 45); W2_46 = W(2, 46); W2_47 = W(2, 47); W2_48 = W(2, 48); W2_49 = W(2, 49); W2_50 = W(2, 50); W2_51 = W(2, 51); W2_52 = W(2, 52); W2_53 = W(2, 53); W2_54 = W(2, 54); W2_55 = W(2, 55); W2_56 = W(2, 56); W2_57 = W(2, 57); W2_58 = W(2, 58); W2_59 = W(2, 59); W2_60 = W(2, 60); W2_61 = W(2, 61); W2_62 = W(2, 62); W2_63 = W(2, 63); W2_64 = W(2, 64); W3_1 = W(3, 1); W3_2 = W(3, 2); W3_3 = W(3, 3); W3_4 = W(3, 4); W3_5 = W(3, 5); W3_6 = W(3, 6); W3_7 = W(3, 7); W3_8 = W(3, 8); W3_9 = W(3, 9); W3_10 = W(3, 10); W3_11 = W(3, 11); W3_12 = W(3, 12); W3_13 = W(3, 13); W3_14 = W(3, 14); W3_15 = W(3, 15); W3_16 = W(3, 16); W3_17 = W(3, 17); W3_18 = W(3, 18); W3_19 = W(3, 19); W3_20 = W(3, 20); W3_21 = W(3, 21); W3_22 = W(3, 22); W3_23 = W(3, 23); W3_24 = W(3, 24); W3_25 = W(3, 25); W3_26 = W(3, 26); W3_27 = W(3, 27); W3_28 = W(3, 28); W3_29 = W(3, 29); W3_30 = W(3, 30); W3_31 = W(3, 31); W3_32 = W(3, 32); W3_33 = W(3, 33); W3_34 = W(3, 34); W3_35 = W(3, 35); W3_36 = W(3, 36); W3_37 = W(3, 37); W3_38 = W(3, 38); W3_39 = W(3, 39); W3_40 = W(3, 40); W3_41 = W(3, 41); W3_42 = W(3, 42); W3_43 = W(3, 43); W3_44 = W(3, 44); W3_45 = W(3, 45); W3_46 = W(3, 46); W3_47 = W(3, 47); W3_48 = W(3, 48); W3_49 = W(3, 49); W3_50 = W(3, 50); W3_51 = W(3, 51); W3_52 = W(3, 52); W3_53 = W(3, 53); W3_54 = W(3, 54); W3_55 = W(3, 55); W3_56 = W(3, 56); W3_57 = W(3, 57); W3_58 = W(3, 58); W3_59 = W(3, 59); W3_60 = W(3, 60); W3_61 = W(3, 61); W3_62 = W(3, 62); W3_63 = W(3, 63); W3_64 = W(3, 64);
    Q1_1 = Q(1, 1); Q1_2 = Q(1, 2); Q1_3 = Q(1, 3); Q1_4 = Q(1, 4); Q2_1 = Q(2, 1); Q2_2 = Q(2, 2); Q2_3 = Q(2, 3); Q2_4 = Q(2, 4); Q3_1 = Q(3, 1); Q3_2 = Q(3, 2); Q3_3 = Q(3, 3); Q3_4 = Q(3, 4);
    [q0_, q1_, q2_, q3_] = solver_func(W1_1, W2_1, W3_1, W1_2, W2_2, W3_2, W1_3, W2_3, W3_3, W1_4, W2_4, W3_4, W1_5, W2_5, W3_5, W1_6, W2_6, W3_6, W1_7, W2_7, W3_7, W1_8, W2_8, W3_8, W1_9, W2_9, W3_9, W1_10, W2_10, W3_10, W1_11, W2_11, W3_11, W1_12, W2_12, W3_12, W1_13, W2_13, W3_13, W1_14, W2_14, W3_14, W1_15, W2_15, W3_15, W1_16, W2_16, W3_16, W1_17, W2_17, W3_17, W1_18, W2_18, W3_18, W1_19, W2_19, W3_19, W1_20, W2_20, W3_20, W1_21, W2_21, W3_21, W1_22, W2_22, W3_22, W1_23, W2_23, W3_23, W1_24, W2_24, W3_24, W1_25, W2_25, W3_25, W1_26, W2_26, W3_26, W1_27, W2_27, W3_27, W1_28, W2_28, W3_28, W1_29, W2_29, W3_29, W1_30, W2_30, W3_30, W1_31, W2_31, W3_31, W1_32, W2_32, W3_32, W1_33, W2_33, W3_33, W1_34, W2_34, W3_34, W1_35, W2_35, W3_35, W1_36, W2_36, W3_36, W1_37, W2_37, W3_37, W1_38, W2_38, W3_38, W1_39, W2_39, W3_39, W1_40, W2_40, W3_40, W1_41, W2_41, W3_41, W1_42, W2_42, W3_42, W1_43, W2_43, W3_43, W1_44, W2_44, W3_44, W1_45, W2_45, W3_45, W1_46, W2_46, W3_46, W1_47, W2_47, W3_47, W1_48, W2_48, W3_48, W1_49, W2_49, W3_49, W1_50, W2_50, W3_50, W1_51, W2_51, W3_51, W1_52, W2_52, W3_52, W1_53, W2_53, W3_53, W1_54, W2_54, W3_54, W1_55, W2_55, W3_55, W1_56, W2_56, W3_56, W1_57, W2_57, W3_57, W1_58, W2_58, W3_58, W1_59, W2_59, W3_59, W1_60, W2_60, W3_60, W1_61, W2_61, W3_61, W1_62, W2_62, W3_62, W1_63, W2_63, W3_63, W1_64, W2_64, W3_64, Q1_1, Q2_1, Q3_1, Q1_2, Q2_2, Q3_2, Q1_3, Q2_3, Q3_3, Q1_4, Q2_4, Q3_4);
elseif(strcmp(char(solver_func), char(@solver_Wu_Qq_sym)))
    W1_1 = W(1, 1); W1_2 = W(1, 2); W1_3 = W(1, 3); W1_4 = W(1, 4); W1_5 = W(1, 5); W1_6 = W(1, 6); W1_7 = W(1, 7); W1_8 = W(1, 8); W1_9 = W(1, 9); W1_10 = W(1, 10); W1_11 = W(1, 11); W1_12 = W(1, 12); W1_13 = W(1, 13); W1_14 = W(1, 14); W1_15 = W(1, 15); W1_16 = W(1, 16); W1_17 = W(1, 17); W1_18 = W(1, 18); W1_19 = W(1, 19); W1_20 = W(1, 20); W1_21 = W(1, 21); W1_22 = W(1, 22); W1_23 = W(1, 23); W1_24 = W(1, 24); W1_25 = W(1, 25); W1_26 = W(1, 26); W1_27 = W(1, 27); W1_28 = W(1, 28); W1_29 = W(1, 29); W1_30 = W(1, 30); W1_31 = W(1, 31); W1_32 = W(1, 32); W1_33 = W(1, 33); W1_34 = W(1, 34); W1_35 = W(1, 35); W1_36 = W(1, 36); W1_37 = W(1, 37); W1_38 = W(1, 38); W1_39 = W(1, 39); W1_40 = W(1, 40); W1_41 = W(1, 41); W1_42 = W(1, 42); W1_43 = W(1, 43); W1_44 = W(1, 44); W1_45 = W(1, 45); W1_46 = W(1, 46); W1_47 = W(1, 47); W1_48 = W(1, 48); W1_49 = W(1, 49); W1_50 = W(1, 50); W1_51 = W(1, 51); W1_52 = W(1, 52); W1_53 = W(1, 53); W1_54 = W(1, 54); W1_55 = W(1, 55); W1_56 = W(1, 56); W1_57 = W(1, 57); W1_58 = W(1, 58); W1_59 = W(1, 59); W1_60 = W(1, 60); W1_61 = W(1, 61); W1_62 = W(1, 62); W1_63 = W(1, 63); W1_64 = W(1, 64); W2_1 = W(2, 1); W2_2 = W(2, 2); W2_3 = W(2, 3); W2_4 = W(2, 4); W2_5 = W(2, 5); W2_6 = W(2, 6); W2_7 = W(2, 7); W2_8 = W(2, 8); W2_9 = W(2, 9); W2_10 = W(2, 10); W2_11 = W(2, 11); W2_12 = W(2, 12); W2_13 = W(2, 13); W2_14 = W(2, 14); W2_15 = W(2, 15); W2_16 = W(2, 16); W2_17 = W(2, 17); W2_18 = W(2, 18); W2_19 = W(2, 19); W2_20 = W(2, 20); W2_21 = W(2, 21); W2_22 = W(2, 22); W2_23 = W(2, 23); W2_24 = W(2, 24); W2_25 = W(2, 25); W2_26 = W(2, 26); W2_27 = W(2, 27); W2_28 = W(2, 28); W2_29 = W(2, 29); W2_30 = W(2, 30); W2_31 = W(2, 31); W2_32 = W(2, 32); W2_33 = W(2, 33); W2_34 = W(2, 34); W2_35 = W(2, 35); W2_36 = W(2, 36); W2_37 = W(2, 37); W2_38 = W(2, 38); W2_39 = W(2, 39); W2_40 = W(2, 40); W2_41 = W(2, 41); W2_42 = W(2, 42); W2_43 = W(2, 43); W2_44 = W(2, 44); W2_45 = W(2, 45); W2_46 = W(2, 46); W2_47 = W(2, 47); W2_48 = W(2, 48); W2_49 = W(2, 49); W2_50 = W(2, 50); W2_51 = W(2, 51); W2_52 = W(2, 52); W2_53 = W(2, 53); W2_54 = W(2, 54); W2_55 = W(2, 55); W2_56 = W(2, 56); W2_57 = W(2, 57); W2_58 = W(2, 58); W2_59 = W(2, 59); W2_60 = W(2, 60); W2_61 = W(2, 61); W2_62 = W(2, 62); W2_63 = W(2, 63); W2_64 = W(2, 64); W3_1 = W(3, 1); W3_2 = W(3, 2); W3_3 = W(3, 3); W3_4 = W(3, 4); W3_5 = W(3, 5); W3_6 = W(3, 6); W3_7 = W(3, 7); W3_8 = W(3, 8); W3_9 = W(3, 9); W3_10 = W(3, 10); W3_11 = W(3, 11); W3_12 = W(3, 12); W3_13 = W(3, 13); W3_14 = W(3, 14); W3_15 = W(3, 15); W3_16 = W(3, 16); W3_17 = W(3, 17); W3_18 = W(3, 18); W3_19 = W(3, 19); W3_20 = W(3, 20); W3_21 = W(3, 21); W3_22 = W(3, 22); W3_23 = W(3, 23); W3_24 = W(3, 24); W3_25 = W(3, 25); W3_26 = W(3, 26); W3_27 = W(3, 27); W3_28 = W(3, 28); W3_29 = W(3, 29); W3_30 = W(3, 30); W3_31 = W(3, 31); W3_32 = W(3, 32); W3_33 = W(3, 33); W3_34 = W(3, 34); W3_35 = W(3, 35); W3_36 = W(3, 36); W3_37 = W(3, 37); W3_38 = W(3, 38); W3_39 = W(3, 39); W3_40 = W(3, 40); W3_41 = W(3, 41); W3_42 = W(3, 42); W3_43 = W(3, 43); W3_44 = W(3, 44); W3_45 = W(3, 45); W3_46 = W(3, 46); W3_47 = W(3, 47); W3_48 = W(3, 48); W3_49 = W(3, 49); W3_50 = W(3, 50); W3_51 = W(3, 51); W3_52 = W(3, 52); W3_53 = W(3, 53); W3_54 = W(3, 54); W3_55 = W(3, 55); W3_56 = W(3, 56); W3_57 = W(3, 57); W3_58 = W(3, 58); W3_59 = W(3, 59); W3_60 = W(3, 60); W3_61 = W(3, 61); W3_62 = W(3, 62); W3_63 = W(3, 63); W3_64 = W(3, 64); W4_1 = W(4, 1); W4_2 = W(4, 2); W4_3 = W(4, 3); W4_4 = W(4, 4); W4_5 = W(4, 5); W4_6 = W(4, 6); W4_7 = W(4, 7); W4_8 = W(4, 8); W4_9 = W(4, 9); W4_10 = W(4, 10); W4_11 = W(4, 11); W4_12 = W(4, 12); W4_13 = W(4, 13); W4_14 = W(4, 14); W4_15 = W(4, 15); W4_16 = W(4, 16); W4_17 = W(4, 17); W4_18 = W(4, 18); W4_19 = W(4, 19); W4_20 = W(4, 20); W4_21 = W(4, 21); W4_22 = W(4, 22); W4_23 = W(4, 23); W4_24 = W(4, 24); W4_25 = W(4, 25); W4_26 = W(4, 26); W4_27 = W(4, 27); W4_28 = W(4, 28); W4_29 = W(4, 29); W4_30 = W(4, 30); W4_31 = W(4, 31); W4_32 = W(4, 32); W4_33 = W(4, 33); W4_34 = W(4, 34); W4_35 = W(4, 35); W4_36 = W(4, 36); W4_37 = W(4, 37); W4_38 = W(4, 38); W4_39 = W(4, 39); W4_40 = W(4, 40); W4_41 = W(4, 41); W4_42 = W(4, 42); W4_43 = W(4, 43); W4_44 = W(4, 44); W4_45 = W(4, 45); W4_46 = W(4, 46); W4_47 = W(4, 47); W4_48 = W(4, 48); W4_49 = W(4, 49); W4_50 = W(4, 50); W4_51 = W(4, 51); W4_52 = W(4, 52); W4_53 = W(4, 53); W4_54 = W(4, 54); W4_55 = W(4, 55); W4_56 = W(4, 56); W4_57 = W(4, 57); W4_58 = W(4, 58); W4_59 = W(4, 59); W4_60 = W(4, 60); W4_61 = W(4, 61); W4_62 = W(4, 62); W4_63 = W(4, 63); W4_64 = W(4, 64); Q11 = Q(1, 1); Q12 = Q(1, 2); Q13 = Q(1, 3); Q14 = Q(1, 4); Q22 = Q(2, 2); Q23 = Q(2, 3); Q24 = Q(2, 4); Q33 = Q(3, 3); Q34 = Q(3, 4); Q44 = Q(4, 4); 
    [q0_, q1_, q2_, q3_] = solver_func(W1_1, W2_1, W3_1, W4_1, W1_2, W2_2, W3_2, W4_2, W1_3, W2_3, W3_3, W4_3, W1_4, W2_4, W3_4, W4_4, W1_5, W2_5, W3_5, W4_5, W1_6, W2_6, W3_6, W4_6, W1_7, W2_7, W3_7, W4_7, W1_8, W2_8, W3_8, W4_8, W1_9, W2_9, W3_9, W4_9, W1_10, W2_10, W3_10, W4_10, W1_11, W2_11, W3_11, W4_11, W1_12, W2_12, W3_12, W4_12, W1_13, W2_13, W3_13, W4_13, W1_14, W2_14, W3_14, W4_14, W1_15, W2_15, W3_15, W4_15, W1_16, W2_16, W3_16, W4_16, W1_17, W2_17, W3_17, W4_17, W1_18, W2_18, W3_18, W4_18, W1_19, W2_19, W3_19, W4_19, W1_20, W2_20, W3_20, W4_20, W1_21, W2_21, W3_21, W4_21, W1_22, W2_22, W3_22, W4_22, W1_23, W2_23, W3_23, W4_23, W1_24, W2_24, W3_24, W4_24, W1_25, W2_25, W3_25, W4_25, W1_26, W2_26, W3_26, W4_26, W1_27, W2_27, W3_27, W4_27, W1_28, W2_28, W3_28, W4_28, W1_29, W2_29, W3_29, W4_29, W1_30, W2_30, W3_30, W4_30, W1_31, W2_31, W3_31, W4_31, W1_32, W2_32, W3_32, W4_32, W1_33, W2_33, W3_33, W4_33, W1_34, W2_34, W3_34, W4_34, W1_35, W2_35, W3_35, W4_35, W1_36, W2_36, W3_36, W4_36, W1_37, W2_37, W3_37, W4_37, W1_38, W2_38, W3_38, W4_38, W1_39, W2_39, W3_39, W4_39, W1_40, W2_40, W3_40, W4_40, W1_41, W2_41, W3_41, W4_41, W1_42, W2_42, W3_42, W4_42, W1_43, W2_43, W3_43, W4_43, W1_44, W2_44, W3_44, W4_44, W1_45, W2_45, W3_45, W4_45, W1_46, W2_46, W3_46, W4_46, W1_47, W2_47, W3_47, W4_47, W1_48, W2_48, W3_48, W4_48, W1_49, W2_49, W3_49, W4_49, W1_50, W2_50, W3_50, W4_50, W1_51, W2_51, W3_51, W4_51, W1_52, W2_52, W3_52, W4_52, W1_53, W2_53, W3_53, W4_53, W1_54, W2_54, W3_54, W4_54, W1_55, W2_55, W3_55, W4_55, W1_56, W2_56, W3_56, W4_56, W1_57, W2_57, W3_57, W4_57, W1_58, W2_58, W3_58, W4_58, W1_59, W2_59, W3_59, W4_59, W1_60, W2_60, W3_60, W4_60, W1_61, W2_61, W3_61, W4_61, W1_62, W2_62, W3_62, W4_62, W1_63, W2_63, W3_63, W4_63, W1_64, W2_64, W3_64, W4_64, Q11, Q12, Q13, Q14, Q22, Q23, Q24, Q33, Q34, Q44);
elseif(strcmp(char(solver_func), char(@solver_WQ_approx)) || ...
       strcmp(char(solver_func), char(@solver_WQ_approx_mex)))
    perm = params{1};
    W = W(perm, :);
    Q = Q(perm, :);
    sols = solver_func([vec(W); vec(Q)]);
    q0_ = sols(1, :);
    q1_ = sols(2, :);
    q2_ = sols(3, :);
    q3_ = sols(4, :);
elseif(strcmp(char(solver_func), char(@solver_WQ)) || ...
       strcmp(char(solver_func), char(@solver_WQ_mex)))
    sols = solver_func([vec(W); vec(Q)]);
    q0_ = sols(1, :);
    q1_ = sols(2, :);
    q2_ = sols(3, :);
    q3_ = sols(4, :);
elseif(strcmp(char(solver_func), char(@solver_WQ_1_2_3_4_5_9_13_17_33_49)) || ...
       strcmp(char(solver_func), char(@solver_WQ_1_2_3_4_5_9_13_17_33_49_mex)))
    unique_idx = [6     7     8    11    12    16    22    23    24    27    28    32    43    44    48    64];
    unique = W(:, unique_idx);
    sols = solver_func([vec(unique); vec(Q)]);
    q0_ = sols(1, :);
    q1_ = sols(2, :);
    q2_ = sols(3, :);
    q3_ = sols(4, :);
elseif(strcmp(char(solver_func), char(@solver_WQ_1_2_3_4_5_9_13_17_33_49_approx)) || ...
       strcmp(char(solver_func), char(@solver_WQ_1_2_3_4_5_9_13_17_33_49_approx2)) || ...
       strcmp(char(solver_func), char(@solver_WQ_1_2_3_4_5_9_13_17_33_49_approx_mex)))
    perm = params{1};
    W = W(perm, :);
    Q = Q(perm, :);
    unique_idx = [6     7     8    11    12    16    22    23    24    27    28    32    43    44    48    64];
    unique = W(:, unique_idx);
    sols = solver_func([vec(unique); vec(Q)]);
    q0_ = sols(1, :);
    q1_ = sols(2, :);
    q2_ = sols(3, :);
    q3_ = sols(4, :);
end
len = size(q0_, 2);
Js = [];
qs = [];
ts = [];
for i = 1 : len
    if(isreal(q0_(i)))
        q0 = real(q0_(i));
        q1 = real(q1_(i));
        q2 = real(q2_(i));
        q3 = real(q3_(i));
        q = [q0; q1; q2; q3];
        q = q ./ norm(q);
        t = t_func(q);
        qs = [qs, q];
        ts = [ts, t];
        Js = [Js; J_func(q, t)];
    end
end
[min, idx] = sort(Js);
R = quat2dcm(qs(:, idx(1)).');
t = ts(:, idx(1));
X = [
    R, t;
    zeros(1, 3), 1];

end